
<!DOCTYPE html>

<html xmlns="http://www.w3.org/1999/xhtml">
  <head>
    <meta charset="utf-8" />
    <title>spatialmath.base.transforms2d &#8212; Spatial Maths package 0.7.0
 documentation</title>
    <link rel="stylesheet" href="../../../_static/alabaster.css" type="text/css" />
    <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
    <link rel="stylesheet" type="text/css" href="../../../_static/graphviz.css" />
    <script id="documentation_options" data-url_root="../../../" src="../../../_static/documentation_options.js"></script>
    <script src="../../../_static/jquery.js"></script>
    <script src="../../../_static/underscore.js"></script>
    <script src="../../../_static/doctools.js"></script>
    <script src="../../../_static/language_data.js"></script>
    <script async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" />
   
  <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
  
  
  <meta name="viewport" content="width=device-width, initial-scale=0.9, maximum-scale=0.9" />

  </head><body>
  

    <div class="document">
      <div class="documentwrapper">
        <div class="bodywrapper">
          

          <div class="body" role="main">
            
  <h1>Source code for spatialmath.base.transforms2d</h1><div class="highlight"><pre>
<span></span><span class="sd">&quot;&quot;&quot;</span>
<span class="sd">This modules contains functions to create and transform rotation matrices</span>
<span class="sd">and homogeneous tranformation matrices.</span>

<span class="sd">Vector arguments are what numpy refers to as ``array_like`` and can be a list,</span>
<span class="sd">tuple, numpy array, numpy row vector or numpy column vector.</span>

<span class="sd">&quot;&quot;&quot;</span>

<span class="c1"># This file is part of the SpatialMath toolbox for Python</span>
<span class="c1"># https://github.com/petercorke/spatialmath-python</span>
<span class="c1"># </span>
<span class="c1"># MIT License</span>
<span class="c1"># </span>
<span class="c1"># Copyright (c) 1993-2020 Peter Corke</span>
<span class="c1"># </span>
<span class="c1"># Permission is hereby granted, free of charge, to any person obtaining a copy</span>
<span class="c1"># of this software and associated documentation files (the &quot;Software&quot;), to deal</span>
<span class="c1"># in the Software without restriction, including without limitation the rights</span>
<span class="c1"># to use, copy, modify, merge, publish, distribute, sublicense, and/or sell</span>
<span class="c1"># copies of the Software, and to permit persons to whom the Software is</span>
<span class="c1"># furnished to do so, subject to the following conditions:</span>
<span class="c1"># </span>
<span class="c1"># The above copyright notice and this permission notice shall be included in all</span>
<span class="c1"># copies or substantial portions of the Software.</span>
<span class="c1"># </span>
<span class="c1"># THE SOFTWARE IS PROVIDED &quot;AS IS&quot;, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR</span>
<span class="c1"># IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,</span>
<span class="c1"># FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE</span>
<span class="c1"># AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER</span>
<span class="c1"># LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,</span>
<span class="c1"># OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE</span>
<span class="c1"># SOFTWARE.</span>

<span class="c1"># Contributors:</span>
<span class="c1"># </span>
<span class="c1">#     1. Luis Fernando Lara Tobar and Peter Corke, 2008</span>
<span class="c1">#     2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)</span>
<span class="c1">#     3. Peter Corke, 2020</span>

<span class="kn">import</span> <span class="nn">sys</span>
<span class="kn">import</span> <span class="nn">math</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">argcheck</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">vectors</span> <span class="k">as</span> <span class="n">vec</span>
<span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">transformsNd</span> <span class="k">as</span> <span class="n">trn</span>
<span class="kn">import</span> <span class="nn">scipy.linalg</span>

<span class="k">try</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="c1">#print(&#39;Using SymPy&#39;)</span>
    <span class="kn">import</span> <span class="nn">sympy</span> <span class="k">as</span> <span class="nn">sym</span>

    <span class="k">def</span> <span class="nf">issymbol</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="k">return</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">sym</span><span class="o">.</span><span class="n">Symbol</span><span class="p">)</span>
<span class="k">except</span> <span class="ne">BaseException</span><span class="p">:</span>
<div class="viewcode-block" id="issymbol"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.issymbol">[docs]</a>    <span class="k">def</span> <span class="nf">issymbol</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="k">return</span> <span class="kc">False</span></div>

<span class="n">_eps</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">finfo</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">float64</span><span class="p">)</span><span class="o">.</span><span class="n">eps</span>


<div class="viewcode-block" id="colvec"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.colvec">[docs]</a><span class="k">def</span> <span class="nf">colvec</span><span class="p">(</span><span class="n">v</span><span class="p">):</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">v</span><span class="p">)</span><span class="o">.</span><span class="n">reshape</span><span class="p">((</span><span class="nb">len</span><span class="p">(</span><span class="n">v</span><span class="p">),</span> <span class="mi">1</span><span class="p">))</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>


<span class="k">def</span> <span class="nf">_cos</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
    <span class="k">if</span> <span class="n">issymbol</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">sym</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>


<span class="k">def</span> <span class="nf">_sin</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
    <span class="k">if</span> <span class="n">issymbol</span><span class="p">(</span><span class="n">theta</span><span class="p">):</span>
        <span class="k">return</span> <span class="n">sym</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="rot2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.rot2">[docs]</a><span class="k">def</span> <span class="nf">rot2</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SO(2) rotation</span>

<span class="sd">    :param theta: rotation angle</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: 2x2 rotation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(2,2)</span>

<span class="sd">    - ``ROT2(THETA)`` is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians.</span>
<span class="sd">    - ``ROT2(THETA, &#39;deg&#39;)`` as above but THETA is in degrees.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">theta</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getunit</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">)</span>
    <span class="n">ct</span> <span class="o">=</span> <span class="n">_cos</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">st</span> <span class="o">=</span> <span class="n">_sin</span><span class="p">(</span><span class="n">theta</span><span class="p">)</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span>
        <span class="p">[</span><span class="n">ct</span><span class="p">,</span> <span class="o">-</span><span class="n">st</span><span class="p">],</span>
        <span class="p">[</span><span class="n">st</span><span class="p">,</span> <span class="n">ct</span><span class="p">]])</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">sym</span><span class="o">.</span><span class="n">Symbol</span><span class="p">):</span>
        <span class="n">R</span> <span class="o">=</span> <span class="n">R</span><span class="o">.</span><span class="n">round</span><span class="p">(</span><span class="mi">15</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">R</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="trot2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.trot2">[docs]</a><span class="k">def</span> <span class="nf">trot2</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;rad&#39;</span><span class="p">,</span> <span class="n">t</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(2) pure rotation</span>

<span class="sd">    :param theta: rotation angle about X-axis</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :param t: translation 2-vector, defaults to [0,0]</span>
<span class="sd">    :type t: array_like    :return: 3x3 homogeneous transformation matrix</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3)</span>

<span class="sd">    - ``TROT2(THETA)`` is a homogeneous transformation (3x3) representing a rotation of</span>
<span class="sd">      THETA radians.</span>
<span class="sd">    - ``TROT2(THETA, &#39;deg&#39;)`` as above but THETA is in degrees.</span>

<span class="sd">    Notes:</span>
<span class="sd">    - Translational component is zero.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">pad</span><span class="p">(</span><span class="n">rot2</span><span class="p">(</span><span class="n">theta</span><span class="p">,</span> <span class="n">unit</span><span class="p">),</span> <span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">),</span> <span class="n">mode</span><span class="o">=</span><span class="s1">&#39;constant&#39;</span><span class="p">)</span>
    <span class="k">if</span> <span class="n">t</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">2</span><span class="p">,</span> <span class="s1">&#39;array&#39;</span><span class="p">)</span>
    <span class="n">T</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="mf">1.0</span>
    <span class="k">return</span> <span class="n">T</span></div>


<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="transl2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.transl2">[docs]</a><span class="k">def</span> <span class="nf">transl2</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Create SE(2) pure translation, or extract translation from SE(2) matrix</span>

<span class="sd">    :param x: translation along X-axis</span>
<span class="sd">    :type x: float</span>
<span class="sd">    :param y: translation along Y-axis</span>
<span class="sd">    :type y: float</span>
<span class="sd">    :return: homogeneous transform matrix or the translation elements of a homogeneous transform</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(3,3)</span>

<span class="sd">    Create a translational SE(2) matrix:</span>

<span class="sd">    - ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a</span>
<span class="sd">      pure translation.</span>
<span class="sd">    - ``T = transl2( V )`` as above but the translation is given by a 2-element</span>
<span class="sd">      list, dict, or a numpy array, row or column vector.</span>


<span class="sd">    Extract the translational part of an SE(2) matrix:</span>

<span class="sd">    P = TRANSL2(T) is the translational part of a homogeneous transform as a</span>
<span class="sd">    2-element numpy array.</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">isscalar</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
        <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">identity</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="p">[</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">]</span>
        <span class="k">return</span> <span class="n">T</span>
    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">2</span><span class="p">):</span>
        <span class="n">T</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">identity</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">T</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="mi">2</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">T</span>
    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)):</span>
        <span class="k">return</span> <span class="n">x</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;bad argument&#39;</span><span class="p">)</span></div>


<div class="viewcode-block" id="ishom2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.ishom2">[docs]</a><span class="k">def</span> <span class="nf">ishom2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if matrix belongs to SE(2)</span>

<span class="sd">    :param T: matrix to test</span>
<span class="sd">    :type T: numpy.ndarray</span>
<span class="sd">    :param check: check validity of rotation submatrix</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: whether matrix is an SE(2) homogeneous transformation matrix</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    - ``ISHOM2(T)`` is True if the argument ``T`` is of dimension 3x3</span>
<span class="sd">    - ``ISHOM2(T, check=True)`` as above, but also checks orthogonality of the rotation sub-matrix and</span>
<span class="sd">      validitity of the bottom row.</span>

<span class="sd">    :seealso: isR, isrot2, ishom, isvec</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="ow">and</span> <span class="n">T</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span> <span class="ow">and</span> <span class="p">(</span><span class="ow">not</span> <span class="n">check</span> <span class="ow">or</span> <span class="p">(</span><span class="n">trn</span><span class="o">.</span><span class="n">isR</span><span class="p">(</span><span class="n">T</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">])</span> <span class="ow">and</span> <span class="n">np</span><span class="o">.</span><span class="n">all</span><span class="p">(</span><span class="n">T</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="p">:]</span> <span class="o">==</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))))</span></div>


<div class="viewcode-block" id="isrot2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.isrot2">[docs]</a><span class="k">def</span> <span class="nf">isrot2</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">False</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Test if matrix belongs to SO(2)</span>

<span class="sd">    :param R: matrix to test</span>
<span class="sd">    :type R: numpy.ndarray</span>
<span class="sd">    :param check: check validity of rotation submatrix</span>
<span class="sd">    :type check: bool</span>
<span class="sd">    :return: whether matrix is an SO(2) rotation matrix</span>
<span class="sd">    :rtype: bool</span>

<span class="sd">    - ``ISROT(R)`` is True if the argument ``R`` is of dimension 2x2</span>
<span class="sd">    - ``ISROT(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.</span>

<span class="sd">    :seealso: isR, ishom2, isrot</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">return</span> <span class="nb">isinstance</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="ow">and</span> <span class="n">R</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">)</span> <span class="ow">and</span> <span class="p">(</span><span class="ow">not</span> <span class="n">check</span> <span class="ow">or</span> <span class="n">trn</span><span class="o">.</span><span class="n">isR</span><span class="p">(</span><span class="n">R</span><span class="p">))</span></div>

<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="trlog2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.trlog2">[docs]</a><span class="k">def</span> <span class="nf">trlog2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Logarithm of SO(2) or SE(2) matrix</span>

<span class="sd">    :param T: SO(2) or SE(2) matrix</span>
<span class="sd">    :type T: numpy.ndarray, shape=(2,2) or (3,3)</span>
<span class="sd">    :return: logarithm</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(2,2) or (3,3)</span>
<span class="sd">    :raises: ValueError</span>

<span class="sd">    An efficient closed-form solution of the matrix logarithm for arguments that are SO(2) or SE(2).</span>

<span class="sd">    - ``trlog2(R)`` is the logarithm of the passed rotation matrix ``R`` which will be</span>
<span class="sd">      2x2 skew-symmetric matrix.  The equivalent vector from ``vex()`` is parallel to rotation axis</span>
<span class="sd">      and its norm is the amount of rotation about that axis.</span>
<span class="sd">    - ``trlog(T)`` is the logarithm of the passed homogeneous transformation matrix ``T`` which will be</span>
<span class="sd">      3x3 augumented skew-symmetric matrix. The equivalent vector from ``vexa()`` is the twist</span>
<span class="sd">      vector (6x1) comprising [v w].</span>


<span class="sd">    :seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa`</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">ishom2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">):</span>
        <span class="c1"># SE(2) matrix</span>

        <span class="k">if</span> <span class="n">trn</span><span class="o">.</span><span class="n">iseye</span><span class="p">(</span><span class="n">T</span><span class="p">):</span>
            <span class="c1"># is identity matrix</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">))</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">return</span> <span class="n">scipy</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">logm</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>

    <span class="k">elif</span> <span class="n">isrot2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="n">check</span><span class="p">):</span>
        <span class="c1"># SO(2) rotation matrix</span>
        <span class="k">return</span> <span class="n">scipy</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">logm</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s2">&quot;Expect SO(2) or SE(2) matrix&quot;</span><span class="p">)</span></div>
<span class="c1"># ---------------------------------------------------------------------------------------#</span>
<div class="viewcode-block" id="trexp2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.trexp2">[docs]</a><span class="k">def</span> <span class="nf">trexp2</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="n">theta</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Exponential of so(2) or se(2) matrix</span>

<span class="sd">    :param S: so(2), se(2) matrix or equivalent velctor</span>
<span class="sd">    :type T: numpy.ndarray, shape=(2,2) or (3,3); array_like</span>
<span class="sd">    :param theta: motion</span>
<span class="sd">    :type theta: float</span>
<span class="sd">    :return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2)</span>
<span class="sd">    :rtype: numpy.ndarray, shape=(2,2) or (3,3)</span>

<span class="sd">    An efficient closed-form solution of the matrix exponential for arguments</span>
<span class="sd">    that are so(2) or se(2).</span>

<span class="sd">    For so(2) the results is an SO(2) rotation matrix:</span>

<span class="sd">    - ``trexp2(S)`` is the matrix exponential of the so(3) element ``S`` which is a 2x2</span>
<span class="sd">      skew-symmetric matrix.</span>
<span class="sd">    - ``trexp2(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is</span>
<span class="sd">      unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude</span>
<span class="sd">      given by ``THETA``.</span>
<span class="sd">    - ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as</span>
<span class="sd">      a 1-vector (array_like).</span>
<span class="sd">    - ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a</span>
<span class="sd">      unit-norm vector representing a rotation axis and a rotation magnitude</span>
<span class="sd">      given by ``THETA``. ``W`` is expressed as a 1-vector (array_like).</span>


<span class="sd">    For se(2) the results is an SE(2) homogeneous transformation matrix:</span>

<span class="sd">    - ``trexp2(SIGMA)`` is the matrix exponential of the se(2) element ``SIGMA`` which is</span>
<span class="sd">      a 3x3 augmented skew-symmetric matrix.</span>
<span class="sd">    - ``trexp2(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA``</span>
<span class="sd">      must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric</span>
<span class="sd">      matrix.</span>
<span class="sd">    - ``trexp2(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as</span>
<span class="sd">      a 3-vector which can be considered a screw motion.</span>
<span class="sd">    - ``trexp2(TW, THETA)`` as above but for an se(2) motion of TW*THETA, where ``TW``</span>
<span class="sd">      must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric</span>
<span class="sd">      matrix.</span>

<span class="sd">     :seealso: trlog, trexp2</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span> <span class="ow">or</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">3</span><span class="p">):</span>
        <span class="c1"># se(2) case</span>
        <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)):</span>
            <span class="c1"># augmentented skew matrix</span>
            <span class="n">tw</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">vexa</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># 3 vector</span>
            <span class="n">tw</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">vec</span><span class="o">.</span><span class="n">iszerovec</span><span class="p">(</span><span class="n">tw</span><span class="p">):</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        
        <span class="k">if</span> <span class="n">theta</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="p">(</span><span class="n">tw</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span> <span class="o">=</span> <span class="n">vec</span><span class="o">.</span><span class="n">unittwist2</span><span class="p">(</span><span class="n">tw</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">assert</span> <span class="n">vec</span><span class="o">.</span><span class="n">isunittwist2</span><span class="p">(</span><span class="n">tw</span><span class="p">),</span> <span class="s1">&#39;If theta is specified S must be a unit twist&#39;</span>

        <span class="n">t</span> <span class="o">=</span> <span class="n">tw</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">]</span>
        <span class="n">w</span> <span class="o">=</span> <span class="n">tw</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span>

        <span class="n">R</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">_rodrigues</span><span class="p">(</span><span class="n">w</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span>

        <span class="n">skw</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">skew</span><span class="p">(</span><span class="n">w</span><span class="p">)</span>
        <span class="n">V</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span> <span class="o">*</span> <span class="n">theta</span> <span class="o">+</span> <span class="p">(</span><span class="mf">1.0</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">theta</span><span class="p">))</span> <span class="o">*</span> <span class="n">skw</span> <span class="o">+</span> <span class="p">(</span><span class="n">theta</span> <span class="o">-</span> <span class="n">math</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">theta</span><span class="p">))</span> <span class="o">*</span> <span class="n">skw</span> <span class="o">@</span> <span class="n">skw</span>

        <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">V</span><span class="nd">@t</span><span class="p">)</span>

    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span> <span class="ow">or</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">isvector</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="mi">1</span><span class="p">):</span>
        <span class="c1"># so(2) case</span>
        <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">S</span><span class="p">,</span> <span class="p">(</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">)):</span>
            <span class="c1"># skew symmetric matrix</span>
            <span class="n">w</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">vex</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1"># 1 vector</span>
            <span class="n">w</span> <span class="o">=</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">getvector</span><span class="p">(</span><span class="n">S</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">theta</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
            <span class="k">assert</span> <span class="n">vec</span><span class="o">.</span><span class="n">isunitvec</span><span class="p">(</span><span class="n">w</span><span class="p">),</span> <span class="s1">&#39;If theta is specified S must be a unit twist&#39;</span>

        <span class="c1"># do Rodrigues&#39; formula for rotation</span>
        <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">_rodrigues</span><span class="p">(</span><span class="n">w</span><span class="p">,</span> <span class="n">theta</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">raise</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s2">&quot; First argument must be SO(2), 1-vector, SE(2) or 3-vector&quot;</span><span class="p">)</span></div>
        
<div class="viewcode-block" id="trinterp2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.trinterp2">[docs]</a><span class="k">def</span> <span class="nf">trinterp2</span><span class="p">(</span><span class="n">T0</span><span class="p">,</span> <span class="n">T1</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">s</span><span class="o">=</span><span class="kc">None</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Interpolate SE(2) matrices</span>

<span class="sd">    :param T0: first SE(2) matrix</span>
<span class="sd">    :type T0: np.ndarray, shape=(3,3)</span>
<span class="sd">    :param T1: second SE(2) matrix</span>
<span class="sd">    :type T1: np.ndarray, shape=(3,3)</span>
<span class="sd">    :param s: interpolation coefficient, range 0 to 1</span>
<span class="sd">    :type s: float</span>
<span class="sd">    :return: SE(2) matrix</span>
<span class="sd">    :rtype: np.ndarray, shape=(3,3)</span>
<span class="sd">    </span>
<span class="sd">    - ``trinterp2(T0, T1, S)`` is a homogeneous transform (3x3) interpolated</span>
<span class="sd">      between T0 when S=0 and T1 when S=1.  T0 and T1 are both homogeneous</span>
<span class="sd">      transforms (3x3).</span>
<span class="sd">    </span>
<span class="sd">    - ``trinterp2(T1, S)`` as above but interpolated between the identity matrix</span>
<span class="sd">      when S=0 to T1 when S=1.</span>
<span class="sd">    </span>
<span class="sd">    Notes:</span>
<span class="sd">        </span>
<span class="sd">    - Rotation angle is linearly interpolated.</span>

<span class="sd">    :seealso: :func:`~spatialmath.base.transforms3d.trinterp`</span>
<span class="sd">    </span>
<span class="sd">    %## 2d homogeneous trajectory</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">if</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">T0</span><span class="p">,</span> <span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="mi">2</span><span class="p">)):</span>
        <span class="c1"># SO(2) case</span>
        <span class="k">if</span> <span class="n">T1</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1">#	TRINTERP2(T, s)</span>
            
            <span class="n">th0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T0</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">T0</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
            
            <span class="n">th</span> <span class="o">=</span> <span class="n">s</span> <span class="o">*</span> <span class="n">th0</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1">#	TRINTERP2(T0, T1, s)</span>
            <span class="k">assert</span> <span class="n">T0</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="n">T1</span><span class="o">.</span><span class="n">shape</span><span class="p">,</span> <span class="s1">&#39;both matrices must be same shape&#39;</span>
        
            <span class="n">th0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T0</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">T0</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
            <span class="n">th1</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T1</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">T1</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
            
            <span class="n">th</span> <span class="o">=</span> <span class="n">th0</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">-</span> <span class="n">s</span><span class="p">)</span> <span class="o">+</span> <span class="n">s</span> <span class="o">*</span> <span class="n">th1</span>
        
        <span class="k">return</span> <span class="n">rot2</span><span class="p">(</span><span class="n">th</span><span class="p">)</span>
    <span class="k">elif</span> <span class="n">argcheck</span><span class="o">.</span><span class="n">ismatrix</span><span class="p">(</span><span class="n">T0</span><span class="p">,</span> <span class="p">(</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="p">)):</span>
        <span class="k">if</span> <span class="n">T1</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1">#	TRINTERP2(T, s)</span>
            
            <span class="n">th0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T0</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">T0</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
            <span class="n">p0</span> <span class="o">=</span> <span class="n">transl2</span><span class="p">(</span><span class="n">T0</span><span class="p">)</span>
            
            <span class="n">th</span> <span class="o">=</span> <span class="n">s</span> <span class="o">*</span> <span class="n">th0</span>
            <span class="n">pr</span> <span class="o">=</span> <span class="n">s</span> <span class="o">*</span> <span class="n">p0</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="c1">#	TRINTERP2(T0, T1, s)</span>
            <span class="k">assert</span> <span class="n">T0</span><span class="o">.</span><span class="n">shape</span> <span class="o">==</span> <span class="n">T1</span><span class="o">.</span><span class="n">shape</span><span class="p">,</span> <span class="s1">&#39;both matrices must be same shape&#39;</span>
        
            <span class="n">th0</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T0</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">T0</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
            <span class="n">th1</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T1</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span><span class="mi">0</span><span class="p">],</span> <span class="n">T1</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">])</span>
            
            <span class="n">p0</span> <span class="o">=</span> <span class="n">transl2</span><span class="p">(</span><span class="n">T0</span><span class="p">)</span>
            <span class="n">p1</span> <span class="o">=</span> <span class="n">transl2</span><span class="p">(</span><span class="n">T1</span><span class="p">)</span>
            
            <span class="n">pr</span> <span class="o">=</span> <span class="n">p0</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">-</span> <span class="n">s</span><span class="p">)</span> <span class="o">+</span> <span class="n">s</span> <span class="o">*</span> <span class="n">p1</span><span class="p">;</span>
            <span class="n">th</span> <span class="o">=</span> <span class="n">th0</span> <span class="o">*</span> <span class="p">(</span><span class="mi">1</span> <span class="o">-</span> <span class="n">s</span><span class="p">)</span> <span class="o">+</span> <span class="n">s</span> <span class="o">*</span> <span class="n">th1</span>
        
        <span class="k">return</span> <span class="n">trn</span><span class="o">.</span><span class="n">rt2tr</span><span class="p">(</span><span class="n">rot2</span><span class="p">(</span><span class="n">th</span><span class="p">),</span> <span class="n">pr</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="ne">ValueError</span><span class="p">(</span><span class="s1">&#39;Argument must be SO(2) or SE(2)&#39;</span><span class="p">)</span></div>


<div class="viewcode-block" id="trprint2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.trprint2">[docs]</a><span class="k">def</span> <span class="nf">trprint2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">label</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">file</span><span class="o">=</span><span class="n">sys</span><span class="o">.</span><span class="n">stdout</span><span class="p">,</span> <span class="n">fmt</span><span class="o">=</span><span class="s1">&#39;</span><span class="si">{:8.2g}</span><span class="s1">&#39;</span><span class="p">,</span> <span class="n">unit</span><span class="o">=</span><span class="s1">&#39;deg&#39;</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Compact display of SO(2) or SE(2) matrices</span>

<span class="sd">    :param T: matrix to format</span>
<span class="sd">    :type T: numpy.ndarray, shape=(2,2) or (3,3)</span>
<span class="sd">    :param label: text label to put at start of line</span>
<span class="sd">    :type label: str</span>
<span class="sd">    :param file: file to write formatted string to</span>
<span class="sd">    :type file: str</span>
<span class="sd">    :param fmt: conversion format for each number</span>
<span class="sd">    :type fmt: str</span>
<span class="sd">    :param unit: angular units: &#39;rad&#39; [default], or &#39;deg&#39;</span>
<span class="sd">    :type unit: str</span>
<span class="sd">    :return: optional formatted string</span>
<span class="sd">    :rtype: str</span>

<span class="sd">    The matrix is formatted and written to ``file`` or if ``file=None`` then the</span>
<span class="sd">    string is returned.</span>

<span class="sd">    - ``trprint2(R)`` displays the SO(2) rotation matrix in a compact</span>
<span class="sd">      single-line format::</span>

<span class="sd">        [LABEL:] THETA UNIT</span>

<span class="sd">    - ``trprint2(T)`` displays the SE(2) homogoneous transform in a compact</span>
<span class="sd">      single-line format::</span>

<span class="sd">        [LABEL:] [t=X, Y;] THETA UNIT</span>

<span class="sd">    Example::</span>

<span class="sd">        &gt;&gt;&gt; T = transl2(1,2)@trot2(0.3)</span>
<span class="sd">        &gt;&gt;&gt; trprint2(a, file=None, label=&#39;T&#39;)</span>
<span class="sd">        &#39;T: t =        1,        2;       17 deg&#39;</span>

<span class="sd">    :seealso: trprint</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">s</span> <span class="o">=</span> <span class="s1">&#39;&#39;</span>

    <span class="k">if</span> <span class="n">label</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
        <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39;</span><span class="si">{:s}</span><span class="s1">: &#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">label</span><span class="p">)</span>

    <span class="c1"># print the translational part if it exists</span>
    <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39;t = </span><span class="si">{}</span><span class="s1">;&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">transl2</span><span class="p">(</span><span class="n">T</span><span class="p">)))</span>

    <span class="n">angle</span> <span class="o">=</span> <span class="n">math</span><span class="o">.</span><span class="n">atan2</span><span class="p">(</span><span class="n">T</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">T</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
    <span class="k">if</span> <span class="n">unit</span> <span class="o">==</span> <span class="s1">&#39;deg&#39;</span><span class="p">:</span>
        <span class="n">angle</span> <span class="o">*=</span> <span class="mf">180.0</span> <span class="o">/</span> <span class="n">math</span><span class="o">.</span><span class="n">pi</span>
    <span class="n">s</span> <span class="o">+=</span> <span class="s1">&#39; </span><span class="si">{}</span><span class="s1"> </span><span class="si">{}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="p">[</span><span class="n">angle</span><span class="p">]),</span> <span class="n">unit</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">file</span><span class="p">:</span>
        <span class="nb">print</span><span class="p">(</span><span class="n">s</span><span class="p">,</span> <span class="n">file</span><span class="o">=</span><span class="n">file</span><span class="p">)</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="k">return</span> <span class="n">s</span></div>


<span class="k">def</span> <span class="nf">_vec2s</span><span class="p">(</span><span class="n">fmt</span><span class="p">,</span> <span class="n">v</span><span class="p">):</span>
    <span class="n">v</span> <span class="o">=</span> <span class="p">[</span><span class="n">x</span> <span class="k">if</span> <span class="n">np</span><span class="o">.</span><span class="n">abs</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="o">&gt;</span> <span class="mi">100</span> <span class="o">*</span> <span class="n">_eps</span> <span class="k">else</span> <span class="mf">0.0</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">v</span><span class="p">]</span>
    <span class="k">return</span> <span class="s1">&#39;, &#39;</span><span class="o">.</span><span class="n">join</span><span class="p">([</span><span class="n">fmt</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">v</span><span class="p">])</span>


<span class="k">try</span><span class="p">:</span>
    <span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
    <span class="kn">from</span> <span class="nn">mpl_toolkits.mplot3d</span> <span class="kn">import</span> <span class="n">Axes3D</span>
    <span class="n">_matplotlib_exists</span> <span class="o">=</span> <span class="kc">True</span>
    
<span class="k">except</span> <span class="ne">BaseException</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="k">def</span> <span class="nf">trplot</span><span class="p">(</span><span class="o">*</span><span class="n">args</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39;** trplot: no plot produced -- matplotlib not installed&#39;</span><span class="p">)</span>
    <span class="n">_matplotlib_exists</span> <span class="o">=</span> <span class="kc">False</span>
        
<span class="k">if</span> <span class="n">_matplotlib_exists</span><span class="p">:</span>

<div class="viewcode-block" id="trplot2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.trplot2">[docs]</a>    <span class="k">def</span> <span class="nf">trplot2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">axes</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">dims</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">frame</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">textcolor</span><span class="o">=</span><span class="kc">None</span><span class="p">,</span> <span class="n">labels</span><span class="o">=</span><span class="p">[</span><span class="s1">&#39;X&#39;</span><span class="p">,</span> <span class="s1">&#39;Y&#39;</span><span class="p">],</span> <span class="n">length</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">arrow</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span> <span class="n">rviz</span><span class="o">=</span><span class="kc">False</span><span class="p">,</span> <span class="n">wtl</span><span class="o">=</span><span class="mf">0.2</span><span class="p">,</span> <span class="n">width</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">d1</span><span class="o">=</span><span class="mf">0.05</span><span class="p">,</span> <span class="n">d2</span><span class="o">=</span><span class="mf">1.15</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Plot a 2D coordinate frame</span>

<span class="sd">        :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame</span>
<span class="sd">        :type: numpy.ndarray, shape=(2,2) or (3,3)</span>
<span class="sd">        :param axes: the axes to plot into, defaults to current axes</span>
<span class="sd">        :type axes: Axes3D reference</span>
<span class="sd">        :param dims: dimension of plot volume as [xmin, xmax, ymin, ymax]</span>
<span class="sd">        :type dims: array_like</span>
<span class="sd">        :param color: color of the lines defining the frame</span>
<span class="sd">        :type color: str</span>
<span class="sd">        :param textcolor: color of text labels for the frame, default color of lines above</span>
<span class="sd">        :type textcolor: str</span>
<span class="sd">        :param frame: label the frame, name is shown below the frame and as subscripts on the frame axis labels</span>
<span class="sd">        :type frame: str</span>
<span class="sd">        :param labels: labels for the axes, defaults to X, Y and Z</span>
<span class="sd">        :type labels: 3-tuple of strings</span>
<span class="sd">        :param length: length of coordinate frame axes, default 1</span>
<span class="sd">        :type length: float</span>
<span class="sd">        :param arrow: show arrow heads, default True</span>
<span class="sd">        :type arrow: bool</span>
<span class="sd">        :param wtl: width-to-length ratio for arrows, default 0.2</span>
<span class="sd">        :type wtl: float</span>
<span class="sd">        :param rviz: show Rviz style arrows, default False</span>
<span class="sd">        :type rviz: bool</span>
<span class="sd">        :param projection: 3D projection: ortho [default] or persp</span>
<span class="sd">        :type projection: str</span>
<span class="sd">        :param width: width of lines, default 1</span>
<span class="sd">        :type width: float</span>
<span class="sd">        :param d1: distance of frame axis label text from origin, default 1.15</span>
<span class="sd">        :type d2: distance of frame label text from origin, default 0.05</span>

<span class="sd">        Adds a 2D coordinate frame represented by the SO(2) or SE(2) matrix to the current axes.</span>

<span class="sd">        - If no current figure, one is created</span>
<span class="sd">        - If current figure, but no axes, a 3d Axes is created</span>

<span class="sd">        Examples:</span>

<span class="sd">             trplot2(T, frame=&#39;A&#39;)</span>
<span class="sd">             trplot2(T, frame=&#39;A&#39;, color=&#39;green&#39;)</span>
<span class="sd">             trplot2(T1, &#39;labels&#39;, &#39;AB&#39;);</span>

<span class="sd">        &quot;&quot;&quot;</span>

        <span class="c1"># TODO</span>
        <span class="c1"># animation</span>
        <span class="c1"># style=&#39;line&#39;, &#39;arrow&#39;, &#39;rviz&#39;</span>

        <span class="c1"># check input types</span>
        <span class="k">if</span> <span class="n">isrot2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">):</span>
            <span class="n">T</span> <span class="o">=</span> <span class="n">trn</span><span class="o">.</span><span class="n">r2t</span><span class="p">(</span><span class="n">T</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="k">assert</span> <span class="n">ishom2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="n">check</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>

        <span class="k">if</span> <span class="n">axes</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
            <span class="c1"># create an axes</span>
            <span class="n">fig</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">gcf</span><span class="p">()</span>
            <span class="k">if</span> <span class="n">fig</span><span class="o">.</span><span class="n">axes</span> <span class="o">==</span> <span class="p">[]:</span>
                <span class="c1"># no axes in the figure, create a 3D axes</span>
                <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">gca</span><span class="p">()</span>

                <span class="k">if</span> <span class="n">dims</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
                    <span class="n">ax</span><span class="o">.</span><span class="n">autoscale</span><span class="p">(</span><span class="n">enable</span><span class="o">=</span><span class="kc">True</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="s1">&#39;both&#39;</span><span class="p">)</span>
                <span class="k">else</span><span class="p">:</span>
                    <span class="k">if</span> <span class="nb">len</span><span class="p">(</span><span class="n">dims</span><span class="p">)</span> <span class="o">==</span> <span class="mi">2</span><span class="p">:</span>
                        <span class="n">dims</span> <span class="o">=</span> <span class="n">dims</span> <span class="o">*</span> <span class="mi">2</span>
                    <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="n">dims</span><span class="p">[</span><span class="mi">0</span><span class="p">:</span><span class="mi">2</span><span class="p">])</span>
                    <span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">dims</span><span class="p">[</span><span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">])</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">set_aspect</span><span class="p">(</span><span class="s1">&#39;equal&#39;</span><span class="p">)</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">set_xlabel</span><span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span>
                <span class="n">ax</span><span class="o">.</span><span class="n">set_ylabel</span><span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="c1"># reuse an existing axis</span>
                <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">gca</span><span class="p">()</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">ax</span> <span class="o">=</span> <span class="n">axes</span>

        <span class="c1"># create unit vectors in homogeneous form</span>
        <span class="n">o</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
        <span class="n">x</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">*</span> <span class="n">length</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">*</span> <span class="n">length</span>

        <span class="c1"># draw the axes</span>

        <span class="k">if</span> <span class="n">rviz</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">5</span> <span class="o">*</span> <span class="n">width</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="s1">&#39;lime&#39;</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">5</span> <span class="o">*</span> <span class="n">width</span><span class="p">)</span>
        <span class="k">elif</span> <span class="n">arrow</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">quiver</span><span class="p">(</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">angles</span><span class="o">=</span><span class="s1">&#39;xy&#39;</span><span class="p">,</span> <span class="n">scale_units</span><span class="o">=</span><span class="s1">&#39;xy&#39;</span><span class="p">,</span> <span class="n">scale</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">,</span> <span class="n">facecolor</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">edgecolor</span><span class="o">=</span><span class="n">color</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">quiver</span><span class="p">(</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">angles</span><span class="o">=</span><span class="s1">&#39;xy&#39;</span><span class="p">,</span> <span class="n">scale_units</span><span class="o">=</span><span class="s1">&#39;xy&#39;</span><span class="p">,</span> <span class="n">scale</span><span class="o">=</span><span class="mi">1</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">,</span> <span class="n">facecolor</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">edgecolor</span><span class="o">=</span><span class="n">color</span><span class="p">)</span>
            <span class="c1"># plot an invisible point at the end of each arrow to allow auto-scaling to work</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">x</span><span class="o">=</span><span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="n">y</span><span class="o">=</span><span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="n">s</span><span class="o">=</span><span class="p">[</span><span class="mi">20</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">plot</span><span class="p">([</span><span class="n">o</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">]],</span> <span class="p">[</span><span class="n">o</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">]],</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">linewidth</span><span class="o">=</span><span class="n">width</span><span class="p">)</span>

        <span class="c1"># label the frame</span>
        <span class="k">if</span> <span class="n">frame</span><span class="p">:</span>
            <span class="k">if</span> <span class="n">textcolor</span> <span class="ow">is</span> <span class="ow">not</span> <span class="kc">None</span><span class="p">:</span>
                <span class="n">color</span> <span class="o">=</span> <span class="n">textcolor</span>

            <span class="n">o1</span> <span class="o">=</span> <span class="n">T</span> <span class="o">@</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="o">-</span><span class="n">d1</span><span class="p">,</span> <span class="o">-</span><span class="n">d1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">o1</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">o1</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="sa">r</span><span class="s1">&#39;$\{&#39;</span> <span class="o">+</span> <span class="n">frame</span> <span class="o">+</span> <span class="sa">r</span><span class="s1">&#39;\}$&#39;</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;top&#39;</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span>

            <span class="c1"># add the labels to each axis</span>

            <span class="n">x</span> <span class="o">=</span> <span class="p">(</span><span class="n">x</span> <span class="o">-</span> <span class="n">o</span><span class="p">)</span> <span class="o">*</span> <span class="n">d2</span> <span class="o">+</span> <span class="n">o</span>
            <span class="n">y</span> <span class="o">=</span> <span class="p">(</span><span class="n">y</span> <span class="o">-</span> <span class="n">o</span><span class="p">)</span> <span class="o">*</span> <span class="n">d2</span> <span class="o">+</span> <span class="n">o</span>

            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">x</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">x</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="s2">&quot;$</span><span class="si">%c</span><span class="s2">_{</span><span class="si">%s</span><span class="s2">}$&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">frame</span><span class="p">),</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span>
            <span class="n">ax</span><span class="o">.</span><span class="n">text</span><span class="p">(</span><span class="n">y</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="n">y</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="s2">&quot;$</span><span class="si">%c</span><span class="s2">_{</span><span class="si">%s</span><span class="s2">}$&quot;</span> <span class="o">%</span> <span class="p">(</span><span class="n">labels</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="n">frame</span><span class="p">),</span> <span class="n">color</span><span class="o">=</span><span class="n">color</span><span class="p">,</span> <span class="n">horizontalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">,</span> <span class="n">verticalalignment</span><span class="o">=</span><span class="s1">&#39;center&#39;</span><span class="p">)</span></div>

    <span class="kn">from</span> <span class="nn">spatialmath.base</span> <span class="kn">import</span> <span class="n">animate</span> <span class="k">as</span> <span class="n">animate</span>
    
<div class="viewcode-block" id="tranimate2"><a class="viewcode-back" href="../../../spatialmath.html#spatialmath.base.transforms2d.tranimate2">[docs]</a>    <span class="k">def</span> <span class="nf">tranimate2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">        Animate a 2D coordinate frame</span>
<span class="sd">    </span>
<span class="sd">        :param T: an SO(2) or SE(2) pose to be displayed as coordinate frame</span>
<span class="sd">        :type: numpy.ndarray, shape=(2,2) or (3,3)</span>
<span class="sd">        :param nframes: number of steps in the animation [defaault 100]</span>
<span class="sd">        :type nframes: int</span>
<span class="sd">        :param repeat: animate in endless loop [default False]</span>
<span class="sd">        :type repeat: bool</span>
<span class="sd">        :param interval: number of milliseconds between frames [default 50]</span>
<span class="sd">        :type interval: int</span>
<span class="sd">        :param movie: name of file to write MP4 movie into</span>
<span class="sd">        :type movie: str</span>
<span class="sd">        </span>
<span class="sd">        Animates a 2D coordinate frame moving from the world frame to a frame represented by the SO(2) or SE(2) matrix to the current axes.</span>
<span class="sd">    </span>
<span class="sd">        - If no current figure, one is created</span>
<span class="sd">        - If current figure, but no axes, a 3d Axes is created</span>
<span class="sd">        </span>
<span class="sd">    </span>
<span class="sd">        Examples:</span>
<span class="sd">    </span>
<span class="sd">             tranimate2(transl(1,2)@trot2(1), frame=&#39;A&#39;, arrow=False, dims=[0, 5])</span>
<span class="sd">             tranimate2(transl(1,2)@trot2(1), frame=&#39;A&#39;, arrow=False, dims=[0, 5], movie=&#39;spin.mp4&#39;)</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">anim</span> <span class="o">=</span> <span class="n">animate</span><span class="o">.</span><span class="n">Animate2</span><span class="p">(</span><span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
        <span class="n">anim</span><span class="o">.</span><span class="n">trplot2</span><span class="p">(</span><span class="n">T</span><span class="p">,</span> <span class="o">**</span><span class="n">kwargs</span><span class="p">)</span>
        <span class="n">anim</span><span class="o">.</span><span class="n">run</span><span class="p">(</span><span class="o">**</span><span class="n">kwargs</span><span class="p">)</span></div>


<span class="k">if</span> <span class="vm">__name__</span> <span class="o">==</span> <span class="s1">&#39;__main__&#39;</span><span class="p">:</span>  <span class="c1"># pragma: no cover</span>
    <span class="kn">import</span> <span class="nn">pathlib</span>
    <span class="kn">import</span> <span class="nn">os.path</span>

    <span class="c1"># trplot2( transl2(1,2), frame=&#39;A&#39;, rviz=True, width=1)</span>
    <span class="c1"># trplot2( transl2(3,1), color=&#39;red&#39;, arrow=True, width=3, frame=&#39;B&#39;)</span>
    <span class="c1"># trplot2( transl2(4, 3)@trot2(math.pi/3), color=&#39;green&#39;, frame=&#39;c&#39;)</span>
    <span class="c1"># plt.grid(True)</span>

    <span class="n">exec</span><span class="p">(</span><span class="nb">open</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">pathlib</span><span class="o">.</span><span class="n">Path</span><span class="p">(</span><span class="vm">__file__</span><span class="p">)</span><span class="o">.</span><span class="n">parent</span><span class="o">.</span><span class="n">absolute</span><span class="p">(),</span> <span class="s2">&quot;test_transforms.py&quot;</span><span class="p">))</span><span class="o">.</span><span class="n">read</span><span class="p">())</span>
</pre></div>

          </div>
          
        </div>
      </div>
      <div class="sphinxsidebar" role="navigation" aria-label="main navigation">
        <div class="sphinxsidebarwrapper">
<p class="logo">
  <a href="../../../index.html">
    <img class="logo" src="../../../_static/../../../../figs/icon.png" alt="Logo"/>
    
  </a>
</p>



<p class="blurb">Spatial maths and geometry for Python</p>




<p>
<iframe src="https://ghbtns.com/github-btn.html?user=petercorke&repo=spatialmath-python&type=watch&count=true&size=large&v=2"
  allowtransparency="true" frameborder="0" scrolling="0" width="200px" height="35px"></iframe>
</p>





<h3>Navigation</h3>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../../intro.html">Introduction</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../spatialmath.html">Classes and functions</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../indices.html">Indices</a></li>
</ul>

<div class="relations">
<h3>Related Topics</h3>
<ul>
  <li><a href="../../../index.html">Documentation overview</a><ul>
  <li><a href="../../index.html">Module code</a><ul>
  </ul></li>
  </ul></li>
</ul>
</div>
<div id="searchbox" style="display: none" role="search">
  <h3 id="searchlabel">Quick search</h3>
    <div class="searchformwrapper">
    <form class="search" action="../../../search.html" method="get">
      <input type="text" name="q" aria-labelledby="searchlabel" />
      <input type="submit" value="Go" />
    </form>
    </div>
</div>
<script>$('#searchbox').show(0);</script>








        </div>
      </div>
      <div class="clearer"></div>
    </div>
    <div class="footer">
      &copy;2020, Peter Corke.
      
      |
      Powered by <a href="http://sphinx-doc.org/">Sphinx 2.4.4</a>
      &amp; <a href="https://github.com/bitprophet/alabaster">Alabaster 0.7.12</a>
      
    </div>

    

    
  </body>
</html>